#!/usr/bin/env python3
# encoding: utf-8
# stm32 python ROS
import os
import time
import threading
import subprocess
import ros_robot_controller_sdk as rrc

led_status_path_current = '/home/pi/wifi_manager/led.txt'
ros_status_path = '/home/pi/wifi_manager/ros_status.txt'
##### MACHINE TYEP SET ######
def set_machine_ros():
    ros_machine_command = "ros2 service call /ros_robot_controller/set_machine_type std_srvs/srv/Trigger '{}'"
    full_command = [
        'docker', 'exec', '-u', 'ubuntu', '-w', '/home/ubuntu', 'MentorPi',
        '/bin/zsh', '-c', f"source /home/ubuntu/.zshrc && {ros_machine_command}"
    ]
    result = subprocess.run(full_command, shell=False, stdout=subprocess.PIPE, stderr=subprocess.PIPE)

def get_machine_type_from_config(config_path):
    try:
        with open(config_path, 'r') as f:
            for line in f:
                line = line.strip()
                match = re.match(r'^\s*export\s+MACHINE_TYPE\s*=\s*["\']?(.*?)["\']?\s*$', line)
                if match:
                    return match.group(1)
            raise ValueError(f"MACHINE_TYPE IS None")
    except FileNotFoundError:
        raise FileNotFoundError(f"CONFIG PATH IS ERROR")


def send_machine_type(board,machine_type):
    if 'Tank' in machine_type:
        motor_type = 0x01
        # battery_level = 0x2710
        battery_level = 0x1af4
    elif 'Acker' in machine_type or 'Mecanum' in machine_type:
        motor_type = 0x02
        battery_level = 0x1af4
    else:
        motor_type = 0x09
        battery_level = 0x1af4
    if motor_type is not None:
        for i in range(2):
            '''
            MOTOR_TYPE_JGB520 0x00
            MOTOR_TYPE_JGB37  0x01
            MOTOR_TYPE_JGB27  0x02
            MOTOR_TYPE_JGB528 0x03
            '''
            board.set_motor_type(motor_type)
            board.set_battery_level(battery_level)
    else:
        print('Please Set the machine_type')

def set_machine_serial(board):
    config_path = "/home/pi/docker/tmp/.typerc"
    machine_type = get_machine_type_from_config(config_path)
    send_machine_type(board,machine_type)
###################################


#############  LED  SET ##############
# ROS发送LED信息
def set_led_ros(led_id, on_time, off_time, repeat=0):
    ros_command = f"ros2 topic pub /ros_robot_controller/set_led ros_robot_controller_msgs/msg/LedState '{{id: {led_id}, on_time: {on_time}, off_time: {off_time}, repeat: {repeat}}}' --once"
    full_command = [
        'docker', 'exec', '-u', 'ubuntu', '-w', '/home/ubuntu', 'MentorPi',
        '/bin/zsh', '-c', f"source /home/ubuntu/.zshrc && {ros_command}"
    ]
    result = subprocess.run(full_command, shell=False, stdout=subprocess.PIPE, stderr=subprocess.PIPE)

# 串口发送 LED
def set_led_serial(led_id, on_time, off_time, repeat=0):
    board = rrc.Board()
    board.set_led(on_time, off_time, repeat=repeat, led_id=led_id)
#################################


############## check ros node #######
def topic_return():
    process = subprocess.Popen(
        ["docker", "exec", '-u', 'ubuntu', '-w', '/home/ubuntu', 'MentorPi', 
        '/bin/zsh', '-c', 'source /home/ubuntu/.zshrc && ros2 topic list'],
        stdout=subprocess.PIPE,
        stderr=subprocess.PIPE
    )
    stdout, stderr = process.communicate(timeout=5)  # 设置超时为5秒
    topics = stdout.decode().split('\n')
    return topics

def check_ros2_control():
    try:
        time.sleep(0.5)
        topics_return = topic_return()
        print('topics_return',topics_return)
        if '/ros_robot_controller/set_led' in topics_return:
            return True
        else:
            return False
    except Exception as e:
        print(f"{e}")


def write_ros_status_file(ros_status):
    with open(ros_status_path, "w") as f:
        f.write(f"ros_status = '{ros_status}'\n")
    print("ros_status =", ros_status)



def read_ros_status_file():
    if not os.path.exists(ros_status_path):
        print("file error")
        return None

    with open(ros_status_path, "r") as f:
        content = f.read()

    local_vars = {}
    try:
        exec(content, {}, local_vars)
        ros_status = local_vars.get("ros_status")
        print("Read Finish ros_status =", ros_status)
        return ros_status
    except Exception as e:
        print("Read fail", e)
        return None

def write_led_message_func(led2_on_t,led2_off_t,path):
    try:
        with open(path, "w") as f:
            f.write(f"led2_on_time = '{led2_on_t}'\n")
            f.write(f"led2_off_time = '{led2_off_t}'\n")
        print(f"led2_on_time =", led2_on_t)
        print(f"led2_off_time =", led2_off_t)
        return True
    except Exception as e:
        print("Read fail", e)
        return None

def read_led_message_file(path):
    if not os.path.exists(path):
        print("file error")
        return None

    with open(path, "r") as f:
        content = f.read()

    local_vars = {}
    try:
        exec(content, {}, local_vars)
        led2_on_time = float(local_vars.get("led2_on_time"))
        led2_off_time = float(local_vars.get("led2_off_time"))
        print("led2_on_time =", led2_on_time, "led2_off_time =", led2_off_time)
        return led2_on_time, led2_off_time
    except Exception as e:
        print("Read fail", e)
        return None
#################################
def mode_change_ros_detect():
    ros_status = check_ros2_control()
    print('ros_staus_output',ros_status)
    if ros_status is not None:
        if ros_status:
            print('Detect the ROS')
            print('Restart Robot')
            time.sleep(5)
            os.system("reboot")
        else:
            print('Detect the ROS')
        write_ros_status_file(ros_status)
    else:
        return -1
        print('ros detect error')
    return ros_status

def main_test():
    global led_status_path_current
    send_message_flag = False
    ros_status_check = check_ros2_control()
    
    if ros_status_check is not None:
        print('ros_status_check',ros_status_check)
        write_ros_status_file(ros_status_check)

    mode = 1 #AP
    ros_staus = read_ros_status_file()

    if mode == 0:
        led2_on_time,led2_off_time = read_led_message_file(led_status_path_current)
        if ros_staus == 'True':
            set_led_ros(2, led2_on_time, led2_off_time, 0)
        else:
            set_led_serial(2, led2_on_time, led2_off_time, 0)  # LED2闪烁

    elif mode == 1:
        led2_on_time,led2_off_time = read_led_message_file(led_status_path_current)
        if ros_staus == 'True':
            set_led_ros(2, led2_on_time, led2_off_time, 0)
        else:
            set_led_serial(2, led2_on_time, led2_off_time, 0)  # LED2闪烁
    print('send_finish')


if __name__ == '__main__':
    # main_test()
    mode_change_ros_detect()
